In Chapter 2 and 3, an introduction to background theory for this thesis is given.
The work presented in this thesis relies on \textbf{probabilistic robotics} (Thrun et al \cite{fox2005probabilistic}), which is introduced in this chapter.
The work also relies on \textbf{computer vision} (Hartley and Zisserman \cite{Hartley2004}), which is introduced in Chapter \ref{chapter:computer-vision}.
A clear vocabulary and corresponding mathematical nomenclature will be established that will be used consistently in the remainder of this thesis.
Probabilistic robotics uses the notation of Thrun et al \cite{fox2005probabilistic}.
%and computer vision uses the notation of Hartley and Zisserman \cite{Hartley2004}.

Sensors are limited in what they can perceive (e.g., the range and resolution is subject to physical limitations).
Sensors are also subject to noise, which deviates sensor measurements in unpredictable ways.
This noise limits the information that can be extracted from the sensor measurements.
Robot actuators (e.g., motors) are also subject to noise, which introduces uncertainty.
Another source of uncertainty is caused by the robot's software, which uses approximate models of the world.
Model errors are a source of uncertainty that has often been ignored in robots.
Robots are real-time systems, limiting the amount of computation that can be done.
This requires the use of algorithmic approximations, but increases the amount of uncertainty even more.

For some robotic applications (e.g., assembly lines with controlled environment), uncertainty is a marginal factor.
However, robots operating in uncontrolled environments (e.g., homes, other planets) will have to cope with significant uncertainty.
Since robots are increasingly deployed in the open world, the issue of uncertainty has become a major challenge for designing capable robots.
\textit{Probabilistic robots} is a relatively new approach to robotics that pays attention to the uncertainty in perception and action.
Uncertainty is represented explicitly using the calculus of probability theory.
This means that probabilistic algorithms represent information by probability distributions over a space.
This allows to represent ambiguity and degree of belief in a mathematically sound way.
Now, robots can make choices (plan) with respect to the uncertainty that remains, or chose to reduce the uncertainty (e.g., explore) if that is the best choice.

\section{Recursive state estimation}
\label{sec:background-recursive-state-estimation}
Environments are characterized by \textit{state}.
A state can be seen as a collection of all aspects of the robot and the environment that can impact the future.
The state also includes variables regarding the robot itself (e.g., pose, velocity, acceleration).
\begin{mydef}
Environments are characterized by \textbf{state}, which describes all aspects of a robot and the environment that can impact the future.
A state can be either \textbf{static state} (non-changing) or \textbf{dynamic state} where certain state variables tend to change over time.
A state is denoted $x$.
The state at time $t$ is denoted $x_t$.
\end{mydef}

\clearpage
Common state variables are:
\begin{itemize}
\item The robot \textit{pose}, which is its location and orientation relative to a global coordinate frame;
\item The \textit{robot velocity}, consisting of a velocity for each pose variable; %A change in attitude is called \textit{angular velocity};
\item The \textit{location and features of surrounding objects in the environment}. The objects can be either \textit{static} or \textit{dynamic}. For some problems, the objects are modeled as \textit{landmarks}, which are distinct, stationary features that provide reliable recognition.
\end{itemize}

A core idea of probabilistic robotics is the estimation of the state from sensor data.
Sensors observe only partial information about those quantities and their measurements are affected by noise. 
State estimation tries to recover state variables from the data.
Instead of computing a single state, a probability distribution is computed over possible world states.
This probability distribution over possible world states is called \textit{belief} and is described on the next page.
\begin{mydef}
\textbf{State estimation} addresses the problem of estimating quantities (state variables) from sensor data that are not directly observable, but can be inferred.
A probability distribution is computed over possible world states.
\end{mydef}

A robot can \textbf{interact} with its environment by influencing the state of its environment through its actuators (\textit{control actions}), or it can gather information about the state through its sensors (\textit{environment sensor measurements}).
\begin{mydef}
$z_{t_1:t_2} = z_{t_1}, z_{t_1 + 1}, z_{t_1 + 2}, \hdots, z_{t_2}$ denotes the set of all \textbf{measurements} acquired from time $t_1$ to time $t_2$, for $t_1 \leq t_2$.
\end{mydef}
\begin{mydef}
$u_{t_1:t_2} = u_{t_1}, u_{t_1 + 1}, u_{t_1 + 2}, \hdots, u_{t_2}$ denotes the sequence of all \textbf{control data} from time $t_1$ to time $t_2$, for $t_1 \leq t_2$.
\end{mydef}
The control data conveys information regarding the change of state.
For example, setting a robot's velocity at $1 \small{m/s}$, suggests that the robot's new position after $2$ seconds is approximately $2$ meters ahead of its position before the command was executed.
As a result of noise, $1.9$ or $2.1$ meters ahead are also likely new positions.
Generally, measurements provide information about the environment's state, increasing the robot's knowledge.
Motion tends to induce a loss of knowledge due to the inherent noise in robot actuation. % and the stochasticity of the environment.

The evolution of state and measurements is performed by \textbf{probabilistic laws}.
State $x_t$ is generated stochastically from the state $x_{t-1}$.
If state $x$ is complete (i.e., best predictor of the future), it is a sufficient summary of all that happened in previous time steps.
This assumption is known as the \textit{Markov Assumption} and is expressed by the following equality:
%and implies the current knowledge is sufficient to represent the past history of the robot.
\begin{equation}
\label{eq:background_state_trans_prob}
p(x_t | x_{0:t - 1}, z_{1:t - 1}, u_{1:t}) = p(x_t | x_{t-1}, u_t)
\end{equation}
where $p(x_t | x_{t-1}, u_t)$ is called \textit{state transition probability} and specifies how environmental state evolves over time as a function of robot control $u_t$.
%The state at time $t$ is stochastically dependent on the state at time $t - 1$ and the control $u$.

\begin{mydef}
The \textbf{Markov Assumption} states that past and future data are independently if one knows the current state $x$. This means the values in any state are only influenced by the values of the state that directly preceded it.
\end{mydef}

The process by which measurements are modeled is expressed by:
\begin{equation}
\label{eq:background_measurement_prob}
p(z_t | x_{0:t}, z_{1:t-1}, u_{1:t}) = p(z_t | x_t)
\end{equation}
where $p(z_t | x_t)$ is called \textit{measurement probability} and specifies how measurements are generated from the environment state $x$.

Another core idea of probabilistic robotics is \textbf{belief}, which reflects the robot's knowledge about the environment's state.
Belief distributions are posterior probabilities over state variables conditioned on the available data.
The Markov Assumption implies the current belief is sufficient to represent the past history of the robot.
%Belief is represented through conditional probability distributions (posterior probabilities over state variables conditioned on the available data).
The belief over state variables is expressed with $bel(x_t)$:
%, which is an abbrevation for the posterior
\begin{equation}
bel(x_t) = p(x_t | z_{1:t}, u_{1:t})
\end{equation}
A \textit{prediction} of the state at time $t$ can be made before incorporating a measurement.
This prediction is used in the context of probabilistic filtering and is denoted as follows:
\begin{equation}
\overline{bel}(x_t) = p(x_t | z_{1:t-1}, u_{1:t})
\end{equation}
This equation predicts the state at time $t$ based on the previous state posterior, before incorporating the measurement at time $t$.
%A \textit{correction} or \textit{measurement update} is obtained by calculating $bel(x_t)$ from $\overline{bel}(x_t)$.

\begin{mydef}
\textbf{Belief} reflects the robot's knowledge about the environment's state, through conditional probability distributions.
Belief over state variable $x_t$ is denoted by $bel(x_t)$.
\end{mydef}

\subsection{Algorithms}
\label{sec:prob-rob-bayes}
The most general algorithm for calculating beliefs is the \textbf{Bayes filter}.
The input of the algorithm is the (initial) belief $bel$ at time $t-1$, the most recent control $u_t$ and measurement $z_t$.
First, a \textit{prediction} for the new belief is computed based on control $u_t$ (the measurement is ignored):
\begin{equation}
\label{eq:background-theory-bayes-predict}
\overline{bel}(x_t) = \int p(x_t | u_t, x_{t-1}) bel(x_{t-1}) dx_{t-1}
\end{equation}
The second step of the Bayes filter is the \textit{measurement update}:
\begin{equation}
\label{eq:background-theory-bayes-measurement}
bel(x_t) = \eta p(z_t | x_t) \overline{bel}(x_t)
\end{equation}
Where $\eta$ is a normalization constant to ensure a valid probability.
Now, $bel(x_t)$ is the robot's belief about the state after the measurement and control data are used to improve the robot's knowledge.
Both steps of the algorithm are performed recursively for all measurements and control commands.

The most popular family of recursive state estimators are the \textbf{Gaussian filters}.
Gaussian filters represent belief by multivariate normal distributions (approximated by a Gaussian function).
The density (probability) over the state space $x$ is characterized by two parameters: the mean $\mu$ and the covariance $\Sigma$.
The dimension of the covariance matrix is the dimensionality of the state $x$ squared.
Representing the posterior by a Gaussian has some important consequences.
First, Gaussians are unimodel and have a single maximum.
However, this is characteristic for many tracking problems, where the posterior is focused around the true state with a certain margin of uncertainty.

In order to implement the filter described above, two additional components are required.
In Section \ref{sec:motion_model}, the \textit{motion model} is described.
In Section \ref{sec:measurement_model}, the \textit{measurement model} is described.

		\section{Motion model}
		\label{sec:motion_model}
Probabilistic motion models describe the relationship between the previous and current pose and the issued controls (commands) by a probability distribution. This is called the \textit{state transition probability}.
The state transition probability plays an essential role in the prediction step of the Bayes filter.
\begin{mydef}
A probabilistic \textbf{motion model} defines the state transition between two consecutive time steps $t-1$ and $t$ after a control action $u_{t}$ has been carried out. It is expressed by the posterior distribution $p (x_t | x_{t-1}, u_{t})$.
\end{mydef}
%In order to derive $p (x_t | x_{t-1}, u_{t})$, it is necessary to derive the kinematics from its mechanics and its %possible sources of noise and deviation.

Two complementary probabilistic motion models are the \textit{Velocity Motion Model} and the \textit{Odometry Motion Model}.
The Velocity Motion Model assumes that a robot is controlled through two velocities: a rotational velocity $\omega_t$ and translational velocity ($v_t$).
\begin{equation}
u_t = 
\left( \begin{array}{c}
v_t \\
\omega_t \end{array} \right)
\end{equation}
Algorithms for computing the probability $p (x_t | x_{t-1}, u_{t})$ can be found in \cite{fox2005probabilistic}.
The Odometry Motion Model assumes that one has access to odometry information (e.g., wheel encoder information).
In practice, odometry models tend to be more accurate, because most robots do not execute velocity command with the level of accuracy that is obtained by measuring odometry.
Technically, odometry readings are not controls because they are received after executing a command.
However, using odometry readings as controls results in a simpler formulation of the estimation problem.

Both motion models are not directly applicable to MAVs.
For example, quadrotor helicopters are controlled through pitch, roll and yaw (as explained in Section \ref{sec:platform-quadrotor-flight-control}).
A conversion between angles to velocities is required to use the Velocity Motion Model for a quadrotor helicopter.
For ground robots, the odometry reading can be obtained by integrating wheel encoder information.
Because a quadrotor helicopter has no contact with the ground, odometry readings are not directly available.
Instead, different sources of information are used to obtain odometry information.
For example, the AR.Drone uses a down-pointing camera to recover odometry information (Section \ref{sec:platform-visual-odometry}).


		\section{Measurement model}
		\label{sec:measurement_model}
Probabilistic measurement models describe the relationship between the world state $x_t$ and how a sensor reading (observation) $z_t$ is formed.
Based on the estimated world state $x_t$, a belief over possible measurements is generated.
Noise in the sensor measurements is modeled explicitly, inherent to the uncertainty of the robot's sensors.
To express the process of generating measurements, a specification of the environment is required.
A map $m$ of the environment is a list of objects and their locations:
\begin{equation}
m = \{m_1, m_2, \hdots, m_N\}
\end{equation}
Where $N$ is the total number of objects in the environment.
Maps are often \textit{features-based maps} or \textit{location-based maps}.
Location-based maps are volumetric and offer a label for any location.
Features-based maps only describe the shape of the environment at specific locations, which are commonly objects.
Features-based maps are more popular because the representation makes it easier to adjust (refine) the position of objects.
\begin{mydef}
A probabilistic \textbf{sensor model} describes the relation between a world state $x_t$ and a sensor reading $z_t$ given an environmental model $m$ in form of a posterior distribution $p (z_t | x_t, m)$.
\end{mydef}


		\section{Localization}
\label{sec:probabilistic-robotics-localization}
\textit{Robot localization} is the problem of estimating the robot's pose (state) relative to a map of the environment.
Localization is an important building block for successful navigation of a robot, together with \textit{perception}, \textit{cognition} and \textit{motion control}.

When a robot is moving in a known environment and starting at a known location, it can keep track of its location by integrating local position estimates (e.g., odometry).
Due to uncertainty of these local estimates, the uncertainty of the robot's absolute location increases over time.
In order to reduce the growing uncertainty, the robot has to retrieve its absolute location by localizing itself in relation to a map.
To do so, the robot uses its sensors to make observations of its environment and relates these observations to a map of the environment.

A probabilistic approach to localization uses belief to represent the estimated global location.
Again, updating the robot's position involves two steps.
The first step is called the \textit{prediction update}.
The robot uses its state at time $t - 1$ and control data $u_t$ to predicts its state at time $t$.
This prediction step increases the uncertainty about the robot's state.
%The uncertainty about the robot's state increases due to integration of the odometric error over time.
The second step is called the \textit{perception update}.  
In this step, the robot uses the information from its sensors to correct the position estimated during the prediction phase, reducing the uncertainty about the robot's state.

\section{Simultaneous Localization and Mapping}
\label{sec:probabilistic-robotics-slam}

A more difficult subclass of localization occurs when a map of the environment is not available.
In this case, the robot's sensor information is used to both recover the robot's path and build a map of the environment.
In the robotics community, this problem is called \textbf{Simultaneous Localization and Mapping (SLAM)}.
A solution to this problem would make a robot truly autonomous.
SLAM is a difficult problem because both the estimated path and constructed map are affected by noise.
Both of them become increasingly inaccurate during travel.
However, when a place that has been mapped is revisited, the uncertainty can be reduced.
This process is called \textit{loop-closing}.
Additionally, the map can be optimized after a loop-closure event is detected.


\subsection{Solution techniques}
\label{sec:background-solution-techniques}
In practice, it is not possible to compute a robot's belief (posterior probabilities) analytically.
%the posterior (belief) analytically.
Therefore, a number of approximation techniques exist. 
This section describes the most popular solution techniques for SLAM problems.

Solution techniques can be divided in two major branches of belief representation: \textit{single-hypothesis belief} and \textit{multi-hypothesis belief}.
\textbf{Single-hypothesis trackers} represent belief by a single world state.
This estimate is associated by a measure of certainty (or variance), allowing the tracker to broaden or narrow the estimated region in the state space.
The main advantage of the single-hypothesis representation is the absence of ambiguity, simplifying decision-making at the robot's cognitive level (e.g., pathplanning).
Due to the absence of ambiguity, the trackers fail to model ambiguities adequately.
%An example is a soccer field, which is a symmetrical environment.

\textbf{Multi-hypothesis trackers} represent belief not just as a single world state, but as a possibly infinite set of states.
The main advantage of the multi-hypothesis representation is that the robot can explicitly maintain uncertainty regarding its state.
This allows a robot to believe in multiple poses simultaneously, allowing the robot to track and reject different hypotheses independently.
One of the main disadvantages of a multi-hypothesis representation involves decision-making.
If the robot represents its position as a set of points, it becomes less obvious to compute the best action.

Furthermore, solution techniques can be divided in two branches of time constraints: \textit{online computing} and \textit{offline computing}.
\textbf{Online} techniques solve a problem in real-time and must guarantee response within strict time constraints.
One advantage of online techniques is the ability to use the response as input for decision-making (e.g., navigation), which is essential for autonomous robots.
Due to the strict time constraints, a limited amount of computations can be performed, which limits the complexity of solution techniques.

\textbf{Offline} techniques have less strict time constraints, which allow increased complexity.
For example, additional refinements %or optimizations
can be performed that would be too slow for online techniques.
One of the main disadvantages of offline techniques involves decision-making.
Because a response is not available in real-time, the robot has to wait or make a decision without incorporating the response.


\subsubsection{Kalman Filter (KF)}
\label{sec:prob-rob-kf}
The most popular technique for implementing a Bayes filter is probably the Kalman Filter (1960) \cite{kalman1960new}, often used for improving vehicle navigation.
The filter has a Single-hypothesis belief representation and can be performed in real-time.
The Kalman Filter is based on the assumption that the system is linear and both the motion model and measurement model are affected by Gaussian noise.
Belief at time $t$ is represented by a multivariate Gaussian distribution defined by its mean $\mu_t$ and covariance $\Sigma_t$.
This means the $x_t$ from the Bayes filter (Equations \eqref{eq:background-theory-bayes-predict} and \eqref{eq:background-theory-bayes-measurement}) has been replaced by $\mu_t$ and $\Sigma_t$.
%This representation limits the filter to continuous states.

The Kalman Filter assumes the system is linear: the state transition $A$ (Equation \ref{eq:background_state_trans_prob}), the motion model $B$ and the sensor model $C$ (Equation \ref{eq:background_measurement_prob}) are linear functions solely depending on the state $x$ or control command $u$, plus a Gaussian noise model $Q$:
\begin{equation}
\label{eq:KF}
\begin{array}{rclr}
\text{\textbf{Predict}} \\
\overline{\mu}_t & = & A_t \mu_{t-1} + B_t u_t  & \text{a-priori mean estimate} \\
\overline{\Sigma}_t & = & A_t \Sigma_{t-1} A_t^T + R_t  & \text{a-priori covariance estimate} \\
\text{\textbf{Update}} \\
K_t & = & \overline{\Sigma}_t C_t^T (C_t \overline{\Sigma}_t C_t^T + Q_t)^{-1} & \text{Kalman gain} \\
\mu_t & = & \overline{\mu}_t + K_t(z_t - C_t \overline{\mu}_t) & \text{updated (a posteriori) state estimate} \\
\Sigma_t & = & (I - K_t C_t) \overline{\Sigma}_t & \text{updated (a posteriori) estimate covariance}
\end{array}
\end{equation}

The Kalman Filter represents the belief $bel(x_t)$ at time $t$ by the mean $\mu_t$ and covariance $\Sigma_t$.
When a measurement is received, a new belief $bel(x_t)$ is predicted based on the previous belief $bel(x_{t-1})$, the control data $u_t$ and both the state transition model and motion model.
This predicted belief describes the most likely state $\overline{\mu}_t$ at time $t$ and the covariance $\overline{\Sigma}_t$.
The measurement is not yet included, because it is a predicted belief.
The update step transforms the predicted belief ($\overline{\mu}_t$, $\overline{\Sigma}_t$) into the desired belief ($\mu_t$, $\Sigma_t$), by incorporating the measurement $z_t$.
The Kalman gain $K_t$ specifies the degree to which the measurement is incorporated into the new state estimate.
The key concept used for Kalman Filtering is the \textit{innovation}, which is the difference between the actual measurement $z_t$ and the expected measurement $C_t \overline{\mu}_t$, which is derived from the predicted state.
A measurement $z_t$ that is far off the predicted measurement, is less reliable, thus less incorporated in the new state estimate.

The standard Kalman Filter requires a linear system (i.e., a linear combination of Gaussians results in another Gaussian), which is insufficient to describe many real-life problems.
Therefore, variations of the original algorithm have been proposed that can cope with different levels of non-linearity.
These variations approximate the motion and sensor models in a way to make them linear again.

\subsubsection{Extended Kalman Filter (EKF)}
\label{sec:prob-rob-ekf}
The Extended Kalman Filter is a variant on the Kalman Filter that can be used for non-linear systems.
It tries to approximate \textit{non-linear} motion and sensor models to make them linear.
The state transition probability and the measurement probabilities are governed by the non-linear functions $g$ and $h$:
\begin{equation}
\begin{array}{rcl}
x_t & = & g(u_t, x_{t-1}) \\
z_t & = & h(x_t) + \delta_t
\end{array}
\end{equation}
These functions replace the matrix operations $A$, $B$ and $C$ of the regular Kalman Filter (Equation \ref{eq:KF}).

They key idea of the EKF approximation is called \textit{linearization}.
A first-order linear Taylor approximation is calculated at the mean of the current belief (Gaussian), resulting in a linear function.
Projecting the Gaussian belief through the linear approximation results in a Gaussian density.
\begin{equation}
\begin{array}{rclr}
\text{\textbf{Predict}} \\
\overline{\mu}_t & = & \boldsymbol{g(u_t, \mu_{t-1})} & \text{a-priori mean estimate} \\
\overline{\Sigma}_t & = & \boldsymbol{G_t} \Sigma_{t-1} \boldsymbol{G_t^T} + R_t  & \text{a-priori covariance estimate} \\
\text{\textbf{Update}} \\
K_t & = & \overline{\Sigma}_t \boldsymbol{H_t^T} (\boldsymbol{H_t} \overline{\Sigma}_t \boldsymbol{H_t^T} + Q_t)^{-1} & \text{Kalman gain} \\
\mu_t & = & \overline{\mu}_t + K_t(z_t - \boldsymbol{h(\overline{\mu}_t)}) & \text{updated (a posteriori) state estimate} \\
\Sigma_t & = & (I - K_t \boldsymbol{H_t}) \overline{\Sigma}_t & \text{updated (a posteriori) estimate covariance}
\end{array}
\end{equation}



\subsubsection{TORO}
\label{sec:prob-rob-toro}
An example of a recent algorithm, especially designed for visual slam, is TORO \cite{grisetti2007efficient}.
As stated in Section \ref{sec:probabilistic-robotics-slam}, both the estimated path and constructed map are affected by noise.
Both of them become increasingly inaccurate during travel.
However, when a place that has been mapped is revisited (loop-closure), the uncertainty can be reduced and the map can be optimized to reduce its error.
This optimization procedure is computationally expensive since it needs to search for a configuration that minimizes the error of the map.

A solution technique for the map optimization problem is TORO. This technique is used in a comparable setting (visual SLAM with a MAV), as described in Section \ref{sec:realted-visual-slam-steder}.
The TORO algorithm operates on a graph-based formulation of the SLAM problem, in which the poses of the robot are modeled by nodes in a graph.
Constraints between poses resulting from observations or from odometry are encoded in the edges between the nodes.
The goal of TORO is to find a configuration of the nodes that maximizes the observation likelihood encoded in the constraints.
Gradient Descent (GD) \cite{olson2006fast} seeks for a configuration of the nodes that maximizes the likelihood of the observations by iteratively selecting an edge (constraint) $< j, i >$ and by moving a set of nodes
%of the network
in order to decrease the error introduced by the selected constraint.

\begin{comment}
The nodes are updated according to the following equation: 
\begin{equation}
x^{t + 1} = x^t + \Delta x
%\lambda \cdot J_{ji}^T \Omega_{ji} r_{ji}
\end{equation}
where $x$ is the set of variables describing the locations of the poses in the network, and the term $\Delta x$ is used to move a node in the direction that decreases the error.
The direction of $\Delta x$ is computed from the residual (opposite of the error vector) scaled by the information encoded in the contraint.
In practice, GD decomposes the overall problem into many smaller problems by optimizing the constraints individually.
\end{comment}

The TORO algorithm uses a tree-based parameterization for describing the configuration of the nodes in the graph.
%Such a tree can be obtained by computing a spanning tree from a graph.
Such a tree can be constructed from trajectory of the robot.
The first node is the root of the tree.
An unique id is assigned to each node based on the timestamps of the observations.
Furthermore, the parent of a node is the node with the smallest id (timestamp) and a constraint between both nodes.
Each node $i$ in the tree is related to a pose $p_i$ in the network and maintains a parameter $x_i$.
The parameter $x_i$ is a 6D vector and describes the relative movement (rotation and translation) from the parent of node $i$ to  node $i$ itself.
%\begin{equation}
%x_i = p_i \ominus p_{parent(i)}
%\end{equation}
The pose of a node can be expressed as:
\begin{equation}
P_i = \prod_{k \in \mathcal{P}_{i,0}} X_k
\end{equation}
Where $\mathcal{P}_{i,0}$ is the ordered list of nodes describing a path in the tree from the root to node $i$.
The homogenous transformation matrix $X_i$ consists of a rotational matrix $R$ and a translational component $t$.

Distributing an error $\epsilon$ over a sequence of $n$ nodes in the two-dimensional space can be done in a straightforward manner.
For example, by changing the pose of the $i$-th node in the chain by $\frac{i}{n} \times r^{2D}$.
In the three-dimensional space, such a technique is not applicable.
The reason for that is the non-commutativity of the three rotations.
The goal of the update rule in GD is to iteratively update the configuration of a set of nodes in order to reduce the error introduced by a constraint.

The error introduced by a constraint is computed as follows:
\begin{equation}
E_{ji} = \Delta_{ji}^{-1} P_i^{-1} P_j
\end{equation}
In TORO's approach, the error reduction is done in two steps.
First, it updates the rotational components $R_k$ of the variables $x_k$ and second, it updates the translational components $t_k$.
The orientation of pose $p_j$ is described by:
\begin{equation}
\mathcal{R}_1\mathcal{R}_2 \hdots \mathcal{R}_n = \mathcal{R}_{1:n}
\end{equation}
where $n$ is the length of the path $\mathcal{P}_{ji}$.
The error can be distributed by determining a set of increments in the intermediate rotations of the chain so that the orientation of the last node $j$ is $\mathcal{R}_{1:n} B$. Here $B$ is a matrix that rotates $x_j$ to the desired orientation based on the error.
Matrix $B$ can be decomposed into a set of incremental rotations $B = B_{1:n}$.
The individual matrices $B_k$ are computed using a spherical linear interpolation (slerp) \cite{barrera2004incremental}.

The translational error is distributed by linearly moving the individual nodes along the path by a fraction of the error.
This fraction depends on the uncertainty of the individual constraints (encoded in the corresponding covariance matrices).

Despite TORO is presented as an efficient estimation method, it cannot solve realistic problems in real-time \cite{grisetti2007efficient}, which makes it an offline solution technique.
All work presented in this thesis has a strong emphasis on online techniques, which allows the work to be used for navigation tasks like the IMAV Pylon challenge (Section \ref{sec:introduction-imav}).
For this reason, a TORO-based optimization algorithm was not used in this thesis.
Nevertheless, the TORO algorithm is described here, because it is being used by Steder's work (as described in Section \ref{sec:realted-visual-slam-steder}).
Furthermore, a single-hypothesis belief representation was chosen for its simplicity.


\begin{comment}
\subsection{Single-hypothesis trackers}
Single-hypothesis trackers represent belief by a single world state.
This estimate is associated by a measure of certainty (or variance), allowing the tracker to broaden or narrow the estimated region in the state space.
The main advantage of the single-hypothesis representation is the absence of ambiguity, simplifying decision-making at the robot's cognitive level (e.g., pathplanning).
Updating the robot's belief is also simplified, because a single state must be updated to a new single state.
However, the main disadvantage of a single-hypothesis representation is that a update process always has to generate a \textit{single} hypothesis, which is challenging and sometimes impossible.

\paragraph{Kalman Filter (KF)}
The most popular technique for implementing a Bayes filter is probably the Kalman Filter (1960) \cite{kalman1960new}, often used for improving vehicle navigation.
The Kalman Filter is based on the assumption that the system is linear and both the motion model and measurement model are affected by white Gaussian noise.
Belief at time $t$ is represented by a multivariate Gaussian distribution defined by its mean $\mu_t$ and covariance $\Sigma_t$.
This representation limits the filter to continuous states.

The Kalman Filter assumes the system is linear: the state transition $A$ (Equation \ref{eq:background_state_trans_prob}), the motion model $B$ and the sensor model $C$ (Equation \ref{eq:background_measurement_prob}) are linear functions solely depending on the state $x$ or control command $u$, plus a Gaussian noise model $Q$:
\begin{equation}
\label{eq:KF}
\begin{array}{rclr}
\text{\textbf{Predict}} \\
\overline{\mu}_t & = & A_t \mu_{t-1} + B_t u_t  & \text{a-priori mean estimate} \\
\overline{\Sigma}_t & = & A_t \Sigma_{t-1} A_t^T + R_t  & \text{a-priori covariance estimate} \\
\text{\textbf{Update}} \\
K_t & = & \overline{\Sigma}_t C_t^T (C_t \overline{\Sigma}_t C_t^T + Q_t)^{-1} & \text{Kalman gain} \\
\mu_t & = & \overline{\mu}_t + K_t(z_t - C_t \overline{\mu}_t) & \text{updated (a posteriori) state estimate} \\
\Sigma_t & = & (I - K_t C_t) \overline{\Sigma}_t & \text{updated (a posteriori) estimate covariance}
\end{array}
\end{equation}

The Kalman Filter presesents the belief $bel(x_t)$ at time $t$ by the mean $\mu_t$ and covariance $\Sigma_t$.
When a measurement is received, a new belief $bel(x_t)$ is predicted based on the previous belief bel($x_{t-1}$), the control data $u_t$ and both the state transition model and motion model.
This predicted belief describes the most likely state $\overline{\mu}_t$ at time $t$ and the covariance $\overline{\Sigma}_t$.
The measurement is not yet included, because its a predicted belief.
The update step transforms the predicted belief ($\overline{\mu}_t$, $\overline{\Sigma}_t$) into the desired belief ($\mu_t$, $\Sigma_t$), by incorporating the measurement $z_t$.
The Kalman gain $K_t$ specifies the degree to which the measurement is incorporated into the new state estimate.
The key concept used for Kalman Filtering is the \textit{innovation}, which is the difference between the actual measurement $z_t$ and the expected measurement $C_t \overline{\mu}_t$, which is derived from the predicted state.
A measurement $z_t$ that is far off the predicted measurement, is less reliable, thus less incorporated in the new state estimate.

The standard Kalman Filter requires a linear system (.i.e, a linear combination of Gaussians results in another Gaussian), which is insufficient to describe many real-life problems.
Therefore, variations of the original algorithm have been proposed that can cope with different levels of non-linearity.
These variations approximate the motion and sensor models in a way to make them linear again.

\paragraph{Extended Kalman Filter (EKF)}
The Extended Kalman Filter is a variant on the Kalman Filter that can be used for non-linear systems.
It tries to approximate \textit{non-linear} motion and sensor models to make them linear.
The state transition probability and the measurement probabilities are governed by the non-linear functions $g$ and $h$:
\begin{equation}
\begin{array}{rcl}
x_t & = & g(u_t, x_{t-1}) \\
z_t & = & h(x_t) + \delta_t
\end{array}
\end{equation}
These functions replace $A$, $B$ and $C$ of the regular Kalman Filter (Equation \ref{eq:KF}).

They key idea of the EKF approximation is called \textit{linearization}.
A first-order linear Taylor approximation is calculated at the mean of the current belief (Gaussian), resulting in a linear function.
Projecting the Gaussian belief through the linear approximation results in a Gaussian density.
\begin{equation}
\begin{array}{rclr}
\text{\textbf{Predict}} \\
\overline{\mu}_t & = & \boldsymbol{g(u_t, \mu_{t-1})} & \text{a-priori mean estimate} \\
\overline{\Sigma}_t & = & \boldsymbol{G_t} \Sigma_{t-1} \boldsymbol{G_t^T} + R_t  & \text{a-priori covariance estimate} \\
\text{\textbf{Update}} \\
K_t & = & \overline{\Sigma}_t \boldsymbol{H_t^T} (\boldsymbol{H_t} \overline{\Sigma}_t \boldsymbol{H_t^T} + Q_t)^{-1} & \text{Kalman gain} \\
\mu_t & = & \overline{\mu}_t + K_t(z_t - \boldsymbol{h(\overline{\mu}_t)}) & \text{updated (a posteriori) state estimate} \\
\Sigma_t & = & (I - K_t \boldsymbol{H_t}) \overline{\Sigma}_t & \text{updated (a posteriori) estimate covariance}
\end{array}
\end{equation}

\paragraph{Unscented Kalman Filter (UKF)}
A drawback of the Extended Kalman Filter is that the true mean and covariance of the posterior distribution is not preserved.
Another variant on the Kalman Filter has been found to yield superior results.
The \textit{Unscented Kalman Filter (UKF)} performs a stochastic linearization using a weighted statistical linear regression process.
The \textit{unscented transform} sampling technique is used to pick a minimal set of sigma points around the mean.
These sigma points are propagated through the non-linear function $g$, from which the mean and covariance of the estimate are then recovered.
Each sigma point has two weights associated with it. One weight is for recovering the mean, the other weight is used for recovering the covariance.

In practice, the EKF is sightly faster than the UKF.
The UKF produces better results than the EKF, depending on the non-linearities and spread of the prior state uncertainty.
However, in many practical applications, the different is very small.


\paragraph{Information Filter (IF)}
The dual of the Kalman Filter is the \textit{Information Filter}.
The key different between both filters lies in the way the Gaussian belief is represented.
Kalman Filters respresent Gaussians by their moments (mean and covariance).
Information Filters present Gaussians by their canonical parameterization (information matrix and information vector).
\begin{equation}
\label{eq:KF}
\begin{array}{rclr}
\text{\textbf{Predict}} \\
\overline{\Omega}_t & = & (A_t \Omega_{t-1}^{-1} A_t^T + R_t)^{-1}  & \text{a-priori information matrix} \\
\overline{\xi}_t & = & \overline{\Omega}_t ( A_t \Omega_{t-1}^{-1} \xi_{t-1} + B_t u_t )  & \text{a-priori information vector} \\
\text{\textbf{Update}} \\
\Omega_t & = & \C_t^T Q_t^{-1} \C_t + \overline{\Omega}_t & \text{updated information matrix} \\
\xi_t & = & C_t^T Q_t^{-1} z_t + \overline{\xi}_t & \text{updated information vector}
\end{array}
\end{equation}

The difference in parameterization leads to different computational complexities.
The prediction step of the Kalman Filter is additive, which makes is computationally inexpensive.
However, the measurement update in Kalman Filter is computationally complex, due to the requirement of matrix inversion.
For the Information Filter, the complexities are exactly opposite.
The prediction step of the Information Filter involves the inversion of two matrices, which can be complex.
On the other hand, the measurement update of the Information Filter is additive, which makes it inexpensive.
The main advantage of the Information Filter is that $N$ measurements can be filtered at each timestep simply by summing their information matrices and vectors.
This makes Information Filters particularly interesting for scenarios where multiple robots have to merge their local information with the information collected by other robots.


\begin{comment}
\subsection{Multi-hypothesis trackers}


\paragraph{Particle Filter}
The key idea of the \textit{Particle Filter}  is to represent the posterior $bel(x_t)$ by a set of random state particles (samples) drawn from this posterior.
This representation is approximate, but can represent a much broader space of discributions than the single-hypothesis trackers.
Another advantage is that the transformations are not limited to linear functions anymore.

The particles (samples) are denoted
\begin{equation}
\chi = x_t^{[1]}, x_t^{[2]}, \hdots, x_t^{[M]}
\end{equation}
Each particle $x_t^{[m]}$ is a state hypothesis at time $t$.
Ideally, the likelihood for a state hypothesis $x_t$ to be in $\chi$, should be proportional to $bel(x_t)$.
The denser a subregion of the space is populated with particles, the more likely it is that the true state falls into this region.
In the prediction step, the motion model is applied to each particle $x_{t-1}^{[m]}$ to generate a prediction $x_t^{[m]}$.
When observation $z_t$ is made, the \textit{importance factor} $w_t^{[m]}$ is computed.
\begin{equation}
w_t^{[m]} = p ( z_t | x_t^{[m]}, z_{0 \to t-1})
\end{equation}
The final step of the particle filter is {resampling}, in which the current set of particles re-sampled based on the computed likelihoods.
The algorithm draws with replacement $M$ particles from the temporary set $\overline{\chi}_t$.
The probability of each particle is given by its importance weights.
This changes the distribution of the particle from $\overline{bel}(x_t)$ to $bel(x_t)$.

Particle filters overcome the limitations of using a Gaussian to represent a probability distribution.
With sufficient samples, they approach the Bayesian optimal estimate, so they can be made more accurate than either the EKF or UKF.
The increased representational power of particle filters comes at the cost of higher computational complexity.
And, when the number of particles is insufficient, the particle filter might suffer from sample impoverishment.
\end{comment}

The theory described in this chapter is used in the work presented in Section \ref{chapter:visual-slam}.
%Section \ref{sec:pose_estimation} describes the Kalman Filter and both the motion model and measurement model.